﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using NIP.ControllerInterface;
using NIP.DynamicControl.Environ;
using NIP.DynamicControl.Constants;

namespace NIP.DynamicControl
{
    public class PVA
    {
        public List<PathPoint> PVAhist;
        public List<DateTime> timehist;
        public TimeSpan dt;
               
        public PVA()
        {
            PVAhist = new List<PathPoint>();
            timehist = new List<DateTime>();
            dt = new TimeSpan();
            PVAhist.Add(new PathPoint());
            PVAhist.Add(new PathPoint());
            PVAhist.Add(new PathPoint());
            timehist.Add(DateTime.Now);
            timehist.Add(DateTime.Now);
            timehist.Add(DateTime.Now);
            
        }

        public void update(Position p)
        {
            PVAhist.Add(new PathPoint(p));
            timehist.Add(DateTime.Now);
            PVAhist.RemoveAt(0);
            timehist.RemoveAt(0);
            dt = timehist[2] - timehist[1];

            for (int c = 0; c < USBC.TotalAxes; c++)
            {
                // update vel for new entry
                PVAhist[2].vel[c] = (PVAhist[2].pos[c] - PVAhist[1].pos[c]) / dt.TotalSeconds;
                //update acc for new entry
                // this needs to be fixed up
                PVAhist[2].acc[c] = (PVAhist[2].vel[c] - PVAhist[1].vel[c]) / dt.TotalSeconds;
            }
        }

        public PathPoint get()
        {
            return PVAhist.Last();
        }
        

        


    }

    

}
